
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_stabilize.c
  * @author     baiyang
  * @date       2021-8-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void stabilize_run(mode_base_t mode);
static ModeNumber stabilize_mode_number(mode_base_t mode);
static bool stabilize_requires_GPS(mode_base_t mode);
static bool stabilize_has_manual_throttle(mode_base_t mode);
static bool stabilize_allows_arming(mode_base_t mode, enum ArmingMethod method);
static bool stabilize_is_autopilot(mode_base_t mode);
static bool stabilize_allows_save_trim(mode_base_t mode);
static bool stabilize_allows_autotune(mode_base_t mode);
static bool stabilize_allows_flip(mode_base_t mode);

static const char *stabilize_name(mode_base_t mode);
static const char *stabilize_name4(mode_base_t mode);
/*----------------------------------variable----------------------------------*/
static struct mode_ops mode_stabilize_ops = {
        .mode_number = stabilize_mode_number,
        .init        = NULL,
        .exit        = NULL,
        .run         = stabilize_run,
        .requires_GPS = stabilize_requires_GPS,
        .has_manual_throttle = stabilize_has_manual_throttle,
        .allows_arming = stabilize_allows_arming,
        .is_autopilot = stabilize_is_autopilot,
        .has_user_takeoff = NULL,
        .in_guided_mode = NULL,
        .logs_attitude = NULL,
        .allows_save_trim = stabilize_allows_save_trim,
        .allows_autotune = stabilize_allows_autotune,
        .allows_flip = stabilize_allows_flip,
        .name = stabilize_name,
        .name4 = stabilize_name4,
        .is_taking_off = NULL,
        .is_landing = NULL,
        .requires_terrain_failsafe = NULL,
        .get_wp = NULL,
        .wp_bearing = NULL,
        .wp_distance = NULL,
        .crosstrack_error = NULL,
        .output_to_motors = NULL,
        .use_pilot_yaw = NULL,
        .throttle_hover = NULL,
        .do_user_takeoff_start = NULL,
        .set_speed_xy = NULL,
        .set_speed_up = NULL,
        .set_speed_down = NULL,
        .pause = NULL,
        .resume = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mode_stabilize_ctor(ModeStabilize* mode_stabilize)
{
    mode_ctor(&mode_stabilize->mode, &mode_stabilize_ops);
}

// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
static void stabilize_run(mode_base_t mode)
{
    // apply simple mode transform to pilot inputs
    // TODO:

    // convert pilot input to lean angles
    float target_roll, target_pitch;
    fms_get_pilot_desired_lean_angles(&target_roll, &target_pitch, fms.g.angle_max, fms.g.angle_max);

    // get pilot's desired yaw rate
    float target_yaw_rate = fms_get_pilot_desired_yaw_rate(rc_norm_input_dz(fms.channel_yaw));

    if (!fms.motors->_armed) {
        // Motors should be Stopped
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_SHUT_DOWN);
    } else if (fms.ap.throttle_zero) {
        // Attempting to Land
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_GROUND_IDLE);
    } else {
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
    }

    switch (fms.motors->_spool_state) {
    case MOTOR_SHUT_DOWN:
        // Motors Stopped
        attctrl_reset_yaw_target_and_rate(fms.attitude_control, true);
        attctrl_reset_rate_controller_I_terms(fms.attitude_control);
        break;

    case MOTOR_GROUND_IDLE:
        // Landed
        attctrl_reset_yaw_target_and_rate(fms.attitude_control, true);
        attctrl_reset_rate_controller_I_terms_smoothly(fms.attitude_control);
        break;

    case MOTOR_THROTTLE_UNLIMITED:
        // clear landing flag above zero throttle
        if (!fms.motors->limit.throttle_lower) {
            fms_set_land_complete(false);
        }
        break;

    case MOTOR_SPOOLING_UP:
    case MOTOR_SPOOLING_DOWN:
        // do nothing
        break;
    }

    // call attitude controller
    attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(fms.attitude_control,target_roll, target_pitch, target_yaw_rate);

    // output pilot's throttle
    attctrl_set_throttle_out(fms.attitude_control, fms_get_pilot_desired_throttle(), 
                                       true, fms.g.throttle_filt);
}

static ModeNumber stabilize_mode_number(mode_base_t mode) { return STABILIZE; }
static bool stabilize_requires_GPS(mode_base_t mode) { return false; }
static bool stabilize_has_manual_throttle(mode_base_t mode) { return true; }
static bool stabilize_allows_arming(mode_base_t mode, enum ArmingMethod method) { return true; };
static bool stabilize_is_autopilot(mode_base_t mode) { return false; }
static bool stabilize_allows_save_trim(mode_base_t mode) { return true; }
static bool stabilize_allows_autotune(mode_base_t mode) { return true; }
static bool stabilize_allows_flip(mode_base_t mode) { return true; }

static const char *stabilize_name(mode_base_t mode) { return "STABILIZE"; }
static const char *stabilize_name4(mode_base_t mode) { return "STAB"; }

/*------------------------------------test------------------------------------*/


